/**
 * @file kalman_filter.c
 * @brief 单变量卡尔曼滤波器实现
 * @author 废话文学创始人
 * @version 1.1
 * @date 2024-07-14
 */

#include "kalman_filter.h"
#include <stdlib.h>
#include <math.h>

/**
 * @brief 初始化卡尔曼滤波器
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 * @param[in] initial_estimate 初始估计值
 * @param[in] error_estimate 初始估计误差
 * @param[in] error_measure 测量误差
 */
void Kalman_Init(Kalman *kalman, float initial_estimate, float error_estimate, float error_measure)
{
    kalman->value.estimate.current = initial_estimate;
    kalman->value.estimate.last = initial_estimate;
    kalman->error.estimate = error_estimate;
    kalman->error.measure = error_measure;
    kalman->error_estimate_buf = error_estimate;
    kalman->Gain = 0;
}

/**
 * @brief 设置卡尔曼滤波器误差参数
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 * @param[in] error_estimate 估计误差
 * @param[in] error_measure 测量误差
 * @note 估计误差动态变化，但会将初始估计误差保留
 * @note 观测误差一般保持不变
 */
void Kalman_Error_Set(Kalman *kalman, float error_estimate, float error_measure)
{
    kalman->error.estimate = error_estimate;
    kalman->error_estimate_buf = error_estimate;
    kalman->error.measure = error_measure;
}

/**
 * @brief 设置观测量
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 * @param[in] value_measure 测量值
 * @note 必须预先设置一次
 */
void Kalman_Value_Measure_Set(Kalman *kalman, float value_measure)
{
    kalman->value.measure = value_measure;
}

/**
 * @brief 更新卡尔曼增益系数
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 */
static void Kalman_Gain_Update(Kalman *kalman)
{
    kalman->Gain = kalman->error_estimate_buf / (kalman->error_estimate_buf + kalman->error.measure);
}

/**
 * @brief 更新当前估计值
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 */
static void Kalman_Value_Estimate_Update(Kalman *kalman)
{
    kalman->value.estimate.last = kalman->value.estimate.current;
    kalman->value.estimate.current = kalman->value.estimate.last + kalman->Gain * (kalman->value.measure - kalman->value.estimate.last);
}

/**
 * @brief 更新估计误差
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 */
static void Kalman_Error_Estimate_Update(Kalman *kalman)
{
    kalman->error_estimate_buf = (1.0f - kalman->Gain) * kalman->error_estimate_buf;
}

/**
 * @brief 重新定位估计值
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 * @note 此函数是实现动态跟踪的关键
 */
static void Kalman_Clear(Kalman *kalman)
{
    kalman->error_estimate_buf = kalman->error.estimate;
}

/**
 * @brief 执行卡尔曼滤波计算
 * @param[in,out] kalman 卡尔曼滤波器结构体指针
 * @param[in] value_measure 测量到的观测值
 * @note 在计算前调用Kalman_Error_Set来设置参数
 * @note 在计算前调用Kalman_Value_Measure_Set来奠定基调
 */
void Kalman_calculate(Kalman *kalman, float value_measure)
{
    Kalman_Value_Measure_Set(kalman, value_measure);
    if (fabsf(kalman->value.estimate.current - kalman->value.measure) >= kalman->error.measure)
    {
        Kalman_Clear(kalman);
    }
    Kalman_Gain_Update(kalman);
    Kalman_Value_Estimate_Update(kalman);
    Kalman_Error_Estimate_Update(kalman);
}

/**
 * @brief 获取当前的估计值
 * @param[in] kalman 卡尔曼滤波器结构体
 * @return 本次计算后的估计值
 * @note 如果实际值不变，估计值应当有收敛的趋势
 */
float Kalman_Value_Estimate_Get(Kalman kalman)
{
    return kalman.value.estimate.current;
}